//
//  JCDTWTool.m
//  JCDTWTool
//
//  Created by 郭吉成 on 2017/11/29.
//  Copyright © 2017年 KOOSPUR. All rights reserved.
//

#import "JCDTWTool.h"


//#define _N 21
//#define _M 20
#define realmax MAXFLOAT

@interface JCDTWTool ()

@property (nonatomic, strong) NSMutableArray *testArray;
@property (nonatomic, strong) NSMutableArray *refArray;

@end

@implementation JCDTWTool

/**
//模板
NSMutableArray *templateArray = [NSMutableArray array];
NSInteger cutIndexSelect = _selectModel.threeDItem.maxSpeedID;
for (JCThreeDModeItem *item in _selectModel.itemArray) {
    [templateArray addObject:item];
}
NSArray *templateVx = [templateArray valueForKey:@"vx"];
NSArray *templateVy = [templateArray valueForKey:@"vy"];
NSArray *templateVz = [templateArray valueForKey:@"vz"];

//我的挥拍
NSMutableArray *myArray = [NSMutableArray array];
NSInteger cutIndexMy = myTemplate.threeDItem.maxSpeedID;
for (JCThreeDModeItem *item in myTemplate.itemArray) {
    [myArray addObject:item];
}
NSArray *myVx = [myArray valueForKey:@"vx"];
NSArray *myVy = [myArray valueForKey:@"vy"];
NSArray *myVz = [myArray valueForKey:@"vz"];

[JCDTWTool dtwWithRefVX:templateVx
                  refVY:templateVy
                  refVZ:templateVz
       refMaxSpeedIndex:cutIndexSelect
          refSwingSpeed:_selectModel.threeDItem.swingSpeed
              andTestVX:myVx
                 testVY:myVy
                 testVZ:myVz
      testMaxSpeedIndex:cutIndexMy
         testSwingSpeed:myTemplate.threeDItem.swingSpeed
    isSameHandDirection:_selectModel.threeDItem.handDirectionType == myTemplate.threeDItem.handDirectionType?YES:NO
               complete:^(CGFloat forwardScore, CGFloat backwardScore, CGFloat mypowerScore, CGFloat myspeedScore, CGFloat myscore) {
                   
                   if (forwardScore < 60) {
                       weakSelf.startF_01.selected = weakSelf.startF_02.selected = weakSelf.startF_03.selected = NO;
                   }else if (60 <= forwardScore && forwardScore < 80){
                       weakSelf.startF_01.selected = YES;
                       weakSelf.startF_02.selected = weakSelf.startF_03.selected = NO;
                   }else if (80 <= forwardScore && forwardScore < 90){
                       weakSelf.startF_01.selected = weakSelf.startF_02.selected = YES;
                       weakSelf.startF_03.selected = NO;
                   }else if (90 <= forwardScore && forwardScore <= 100){
                       weakSelf.startF_01.selected = weakSelf.startF_02.selected = weakSelf.startF_03.selected = YES;
                   }
                   
                   if (backwardScore < 60) {
                       weakSelf.startB_01.selected = weakSelf.startB_02.selected = weakSelf.startB_03.selected = NO;
                   }else if (60 <= backwardScore && backwardScore < 80){
                       weakSelf.startB_01.selected = YES;
                       weakSelf.startB_02.selected = weakSelf.startB_03.selected = NO;
                   }else if (80 <= backwardScore && backwardScore < 90){
                       weakSelf.startB_01.selected = weakSelf.startB_02.selected = YES;
                       weakSelf.startB_03.selected = NO;
                   }else if (90 <= backwardScore && backwardScore <= 100){
                       weakSelf.startB_01.selected = weakSelf.startB_02.selected = weakSelf.startB_03.selected = YES;
                   }
                   
                   if (mypowerScore < 60) {
                       weakSelf.startP_01.selected = weakSelf.startP_02.selected = weakSelf.startP_03.selected = NO;
                   }else if (60 <= mypowerScore && mypowerScore < 80){
                       weakSelf.startP_01.selected = YES;
                       weakSelf.startP_02.selected = weakSelf.startP_03.selected = NO;
                   }else if (80 <= mypowerScore && mypowerScore < 90){
                       weakSelf.startP_01.selected = weakSelf.startP_02.selected = YES;
                       weakSelf.startP_03.selected = NO;
                   }else if (90 <= mypowerScore && mypowerScore <= 100){
                       weakSelf.startP_01.selected = weakSelf.startP_02.selected = weakSelf.startP_03.selected = YES;
                   }
                   
                   if (myspeedScore < 60) {
                       weakSelf.startS_01.selected = weakSelf.startS_02.selected = weakSelf.startS_03.selected = NO;
                   }else if (60 <= myspeedScore && myspeedScore < 80){
                       weakSelf.startS_01.selected = YES;
                       weakSelf.startS_02.selected = weakSelf.startS_03.selected = NO;
                   }else if (80 <= myspeedScore && myspeedScore < 90){
                       weakSelf.startS_01.selected = weakSelf.startS_02.selected = YES;
                       weakSelf.startS_03.selected = NO;
                   }else if (90 <= myspeedScore && myspeedScore <= 100){
                       weakSelf.startS_01.selected = weakSelf.startS_02.selected = weakSelf.startS_03.selected = YES;
                   }
                   
                   [weakSelf.scoreLabel countFrom:0 to:myscore withDuration:1];
                   if (90 < myscore && myscore<= 100) {
                       weakSelf.evaluateLabel.text = @"完美";
                   }else if (80 < myscore && myscore<= 90) {
                       weakSelf.evaluateLabel.text = @"优秀";
                   }else if (60 < myscore && myscore<= 80) {
                       weakSelf.evaluateLabel.text = @"良好";
                   }else if (0 <= myscore && myscore<= 60) {
                       weakSelf.evaluateLabel.text = @"一般";
                   }else{
                       weakSelf.evaluateLabel.text = @"出错";
                   }
                   if (weakSelf.selectModel.mainType == TemplateMainTypeMy) {
                       myTemplate.matchScore = myscore;
                       [weakSelf.selectModel addMatchSwing:myTemplate];
                   }
               }];
 
 */

/**
 *  DTW计算
 *
 *  @param refVX                模板VX轴数组
 *  @param refVY                模板VY轴数组
 *  @param refVZ                模板VZ轴数组
 *  @param refMaxSpeedIndex     模板最大速度序号。不传：默认取中间
 *  @param refSwingSpeed        模板挥拍速度
 *  @param testVX               我的VX轴数组
 *  @param testVY               我的VY轴数组
 *  @param testVZ               我的VY轴数组
 *  @param testMaxSpeedIndex    我的最大速度序号。不传：默认取中间
 *  @param testSwingSpeed       我的挥拍速度
 *  @param isSameHandDirection  是否相同正反手
 *  @param complete             计算结果回调：forwardScore - 引拍阶段分值
 *                                        ：backwardScore - 收拍阶段分值
 *                                        ：powerScore - 挥拍力度分值
 *                                        ：speedScore - 挥拍速度分值
 *                                        ：score - 最终分值
 */
+ (void)dtwWithRefVX:(NSArray *)refVX
               refVY:(NSArray *)refVY
               refVZ:(NSArray *)refVZ
    refMaxSpeedIndex:(NSInteger)refMaxSpeedIndex
       refSwingSpeed:(NSInteger)refSwingSpeed
           andTestVX:(NSArray *)testVX
              testVY:(NSArray *)testVY
              testVZ:(NSArray *)testVZ
   testMaxSpeedIndex:(NSInteger)testMaxSpeedIndex
      testSwingSpeed:(NSInteger)testSwingSpeed
 isSameHandDirection:(BOOL)isSameHandDirection
            complete:(void(^)(CGFloat forwardScore, CGFloat backwardScore, CGFloat powerScore,CGFloat speedScore, CGFloat score)) complete{
    
    dispatch_async(dispatch_get_global_queue(DISPATCH_QUEUE_PRIORITY_DEFAULT, 0), ^{
        
        float resultF = 0;
        float resultB = 0;
        float powerScore = 0;
        float speedScore = 0;
        float score = 0;
        float testmaxVz = 0;
        float refmaxVz = 0;
        NSInteger refIndex = refMaxSpeedIndex;
        NSInteger testIndex = testMaxSpeedIndex;
        
        if (refIndex == 0) {
            refIndex = ((NSArray *)refVX).count / 2;
        }
        if (testIndex == 0) {
            testIndex = ((NSArray *)testVX).count / 2;
        }
        
        NSMutableArray *refFVXArray = [NSMutableArray array];
        NSMutableArray *refFVYArray = [NSMutableArray array];
        NSMutableArray *refFVZArray = [NSMutableArray array];
        NSMutableArray *refBVXArray = [NSMutableArray array];
        NSMutableArray *refBVYArray = [NSMutableArray array];
        NSMutableArray *refBVZArray = [NSMutableArray array];
        for (NSInteger i = 0; i < refVX.count; i++) {
            if (i < refIndex) {
                [refFVXArray addObject:refVX[i]];
                [refFVYArray addObject:refVY[i]];
                [refFVZArray addObject:refVZ[i]];
            }else{
                [refBVXArray addObject:refVX[i]];
                [refBVYArray addObject:refVY[i]];
                [refBVZArray addObject:refVZ[i]];
            }
            if (i == refIndex) {
                refmaxVz = [refVZ[i] floatValue];
            }
        }
        
        NSMutableArray *testFVXArray = [NSMutableArray array];
        NSMutableArray *testFVYArray = [NSMutableArray array];
        NSMutableArray *testFVZArray = [NSMutableArray array];
        NSMutableArray *testBVXArray = [NSMutableArray array];
        NSMutableArray *testBVYArray = [NSMutableArray array];
        NSMutableArray *testBVZArray = [NSMutableArray array];
        for (NSInteger i = 0; i < testVX.count; i++) {
            if (i < testIndex) {
                [testFVXArray addObject:testVX[i]];
                [testFVYArray addObject:testVY[i]];
                [testFVZArray addObject:testVZ[i]];
            }else{
                [testBVXArray addObject:testVX[i]];
                [testBVYArray addObject:testVY[i]];
                [testBVZArray addObject:testVZ[i]];
            }
            if (i == testIndex) {
                testmaxVz = [testVZ[i] floatValue];
            }
        }
        
        NSArray *testFArray = @[testFVXArray,testFVYArray,testFVZArray];
        NSArray *refFArray = @[refFVXArray,refFVYArray,refFVZArray];
        NSArray *testBArray = @[testBVXArray,testBVYArray,testBVZArray];
        NSArray *refBArray = @[refBVXArray,refBVYArray,refBVZArray];
        
        ///前半拍
        JCDTWTool *toolF = [[JCDTWTool alloc] initWithTestArray:testFArray refArray:refFArray];
        resultF = [toolF calculateDTW];
        ///后半拍
        JCDTWTool *toolB = [[JCDTWTool alloc] initWithTestArray:testBArray refArray:refBArray];
        resultB = [toolB calculateDTW];
        ///发力阶段计算
        powerScore = (1 - fabsf(testmaxVz - refmaxVz)/80.0)*100 * (isSameHandDirection?1:0.5);
        if (powerScore < 0) {
            powerScore = 0;
        }
        ///拍速评分
        speedScore = (1 - labs(testSwingSpeed - refSwingSpeed)/100.0)*100;
        if (speedScore < 0) {
            speedScore = 0;
        }
        //最终得分
        score = powerScore * 0.4 +
        resultF * 0.25 +
        resultB * 0.25 +
        speedScore * 0.1;
        
        if (complete) {
            dispatch_async(dispatch_get_main_queue(), ^{
                complete(resultF, resultB, powerScore, speedScore, score);
            });
        }
    });
}

- (instancetype)initWithTestArray:(NSArray *)testArray refArray:(NSArray *)refArray{
    if (self = [super init]) {
        _testArray = [NSMutableArray arrayWithArray:testArray];
        _refArray = [NSMutableArray arrayWithArray:refArray];
    }
    return self;
}

- (float)calculateDTW{
    
    int _N_T = (int)((NSArray *)_testArray.firstObject).count;
    int _M_R = (int)((NSArray *)_refArray.firstObject).count;
    
    float MArray [_M_R][3];
    float NArray [_N_T][3];
    float _d [_N_T][_M_R];
    float _D [_N_T][_M_R];
    float _D1 = 0;
    float _D2 = 0;
    float _D3 = 0;
    int squeCount = 0;
    
    //数据整理
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < _N_T; j++)
        {
            NArray[j][i] = [_testArray[i][j] floatValue];
        }
    }
    
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < _M_R; j++)
        {
            MArray[j][i] = [_refArray[i][j] floatValue];
        }
    }
    
    for (int i = 0; i < _N_T; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            //                printf("Narray--->>>>[%d][%d] %f\n",i,j,NArray[i][j]);
        }
    }
    
    //帧匹配距离矩阵
    for (int _in = 0; _in < _N_T; _in++)
    {
        float _nx = NArray[_in][0];
        float _ny = NArray[_in][1];
        float _nz = NArray[_in][2];
        for (int _im = 0; _im < _M_R; _im++)
        {
            float _mx = MArray[_im][0];
            float _my = MArray[_im][1];
            float _mz = MArray[_im][2];
            
            float _squera = (_mx - _nx) * (_mx - _nx) + (_my - _ny) * (_my - _ny) + (_mz - _nz) * (_mz - _nz);
            float _disvalue = sqrtf(_squera);
            squeCount++;
            _d[_in][_im] = _disvalue;
            //            printf(" ===< [%d] -> %f\n",squeCount,_disvalue);
        }
    }
    
    for (int i = 0; i < _N_T; i++) {
        for (int j = 0; j < _M_R; j++) {
            _D[i][j] = realmax;
            _D[0][0] = _d[0][0];
            //            printf("_d[%d][%d] = %f      _D[%d][%d] = %f\n",i,j,_d[i][j],i,j,_D[i][j]);
        }
    }
    
    //动态规划
    for (int _i = 1; _i < _N_T; _i++) {
        for (int _m = 0; _m < _M_R; _m++) {
            
            _D1 = _D[_i - 1][_m];
            
            if (_m > 0) {
                _D2 = _D[_i - 1][_m-1];
            }
            else{
                _D2 = realmax;
            }
            
            if (_m > 1) {
                _D3 = _D[_i - 1][_m - 2];
            }
            else{
                _D3 = realmax;
            }
            
            _D[_i][_m] = _d[_i][_m] + zmin(_D1, _D2, _D3);
        }
    }
    
    //    printf("\n\n******************************\n\n");
    
    for (int i = 0; i < _N_T; i++) {
        for (int j = 0; j < _M_R; j++) {
            //            printf("%f  ",_D[i][j]);
        }
        //        printf("\n\n\n");
    }
    
    float result = _D[_N_T - 1][_M_R - 1];
    
    if (result < 10000000) {
        if (result == 0) {
            result = 100;
        } else if (result >= 10000) {
            result = 0;
        } else {
            float dasw = (float) result / 10000.0f;
            float dsw = 1 - dasw;
            result = dsw * 100;
        }
    }else{
        result = 0;
    }
    
    //    printf("\n\n 计算结果:%f -- %f\n\n",_D[_N_T - 1][_M_R - 1],result);
    return result;
}

/**
 求D3，D4，D5的最小值
 **/
float zmin(float a,float b, float c)
{
    float w, aa;
    aa = realmax;
    
    if(aa == b && aa == c && aa == a)
        return w = 8.8e38;
    
    if(a > b)
    {
        if(b >= c)
            w = c;
        else
            w = b;
    }
    else
    {
        if(a >= c)
            w = c;
        else
            w = a;
    }
    
    if(a == b)
    {
        if(a <= c)
            w = a;
        else w = c;
    }
    
    return w;
}

@end
